# import numpy as np
# from scipy.fftpack import fft, ifft, fftshift
# import matplotlib.pyplot as plt
# from scipy import signal

# def getL(data):
#     if len(data) % 2 == 1:
#         data = data[1:,:]
        

#     X = fft(data[:, 0])
#     X = fftshift(X)
#     X = np.abs(X)

#     fftNum = len(X)
#     bias = 1
#     X = X[round(fftNum / 2) + bias:]

#     # plt.plot(X)
#     # plt.show()

#     # 采样频率
#     fs = 30
#     # 单摆频率
#     E1Idx = np.argmax(X)
#     freq_fix = 0
#     E1 = X[E1Idx]
#     E2 = X[E1Idx - 1]
#     E3 = X[E1Idx + 1]
#     if(E2 > E3):
#         freq_fix = (E2 - E3) / (E1 - E3) * fs / fftNum * 0.5
#     else:
#         freq_fix = -(E2 - E3) / (E1 - E2) * fs / fftNum * 0.5

#     print(E1Idx)
#     f = fs / fftNum * (E1Idx + bias) - freq_fix



#     # print(f)
#     # 长度
#     L = (1 / (2 * np.pi * f))**2 * 9.80665

#     return L

# # data = np.loadtxt('/home/pi/workspace/stereo-cv/data/points.txt')

# # # plt.plot(data[:, 0])
# # # plt.show()

# # L = getL(data)

# # print(L)

# # peaks = signal.find_peaks(data[:, 0], distance=50)

import numpy as np
from scipy.fftpack import fft, ifft, fftshift
import matplotlib.pyplot as plt
from scipy import signal

def getL(data):
    dist = 10
    peaks = signal.find_peaks(data[:, 0], distance=dist)
    totalT = 0
    for i in  range(2, len(peaks[0])):
        totalT += data[peaks[0][i], 2] - data[peaks[0][i - 1], 2]
    T0 = totalT / (len(peaks[0]) - 2)

    f0 = 1 / T0
    L0 = (1 / (2 * np.pi * f0))**2 * 9.80665

    peaks = signal.find_peaks(-data[:, 0], distance=dist)
    totalT = 0
    for i in  range(2, len(peaks[0])):
        totalT += data[peaks[0][i], 2] - data[peaks[0][i - 1], 2]
    T1 = totalT / (len(peaks[0]) - 2)
    f1 = 1 / T1
    L1 = (1 / (2 * np.pi * f1))**2 * 9.80665

    f = 0
    minL = 0.4
    maxL = 1.6
    if L0 < maxL and L0 > minL and L1 < maxL and L1 > minL:
        f = (f0 + f1) /  2
    elif L0 < maxL and L0 > minL and (L1 > maxL or L1 < minL):
        f = f0
    elif L1 < maxL and L1 > minL and (L0 > maxL or L0 < minL):
        f = f1
    if f != 0:
        L = (1 / (2 * np.pi * f))**2 * 9.80665
        # print("L: " + str(L))
    else:
        # print("error")
        L = 0
    # peaklist = np.array(data[:, 0])[peaks[0][0:]]
    # plt.plot(data[peaks[0], 2], peaklist, '*')
    # plt.plot(data[:, 2], data[:, 0])
    # plt.show()
    return L

if __name__ == '__main__':
    data = np.loadtxt('C:\\1A\\Projects\\电赛\\data\\points.txt')
    print(getL(data))




